#include "utility.h"

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>

using namespace gtsam;

using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G;  // GPS pose
using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

#include "lidarFactorGtsam.hpp"
#include "nano_gicp/nanoflann.hpp"
#include "nano_gicp/nano_gicp.hpp"
#include "nano_gicp/lsq_registration.hpp"

/*
 * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is
 * time stamp)
 */
struct PointXYZIRPYT {
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIRPYT,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
        float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)(double, time,
                                                                 time))

typedef PointXYZIRPYT PointTypePose;

class lidarOdometry : public ParamServer {
public:
    // gtsam
    NonlinearFactorGraph gtSAMgraph;
    Values initialEstimate;
    Values optimizedEstimate;
    ISAM2* isam;
    Values isamCurrentEstimate;
    Eigen::MatrixXd poseCovariance;

    // Imu Preintegration
    gtsam::PreintegratedImuMeasurements* imuIntegratorOpt_;
    gtsam::PreintegratedImuMeasurements* imuIntegratorImu_;
    gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
    gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
    gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
    gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
    gtsam::Vector noiseModelBetweenBias;
    gtsam::Pose3 prevPose_;
    gtsam::Vector3 prevVel_;
    gtsam::NavState prevState_;
    gtsam::imuBias::ConstantBias prevBias_;
    gtsam::NavState prevStateOdom;
    gtsam::imuBias::ConstantBias prevBiasOdom;

    std::deque<sensor_msgs::Imu> imuQueOpt;
    std::deque<sensor_msgs::Imu> imuQueImu;

    // Publisher and Subscriber
    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubOdomAftMappedROS;
    ros::Publisher pubKeyPoses;
    ros::Publisher pubPath;

    ros::Publisher pubRecentKeyFrames;
    ros::Publisher pubRecentKeyFrame;
    ros::Publisher pubCloudRegisteredRaw;

    ros::Subscriber subImu;
    ros::Subscriber subLaserCloudInfo;

    std::vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    std::vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;

    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;

    pcl::PointCloud<PointType>::Ptr
        laserCloudCornerLast;  // corner feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr
        laserCloudSurfLast;  // surf feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr
        laserCloudCornerLastDS;  // downsampled corner featuer set from
                                 // odoOptimization
    pcl::PointCloud<PointType>::Ptr
        laserCloudSurfLastDS;  // downsampled surf featuer set from
                               // odoOptimization

    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;

    std::vector<PointType>
        laserCloudOriCornerVec;  // corner point holder for parallel computation
    std::vector<PointType> coeffSelCornerVec;
    std::vector<bool> laserCloudOriCornerFlag;
    std::vector<PointType>
        laserCloudOriSurfVec;  // surf point holder for parallel computation
    std::vector<PointType> coeffSelSurfVec;
    std::vector<bool> laserCloudOriSurfFlag;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;

    nanoflann::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
    nanoflann::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;

    nanoflann::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
    nanoflann::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;

    pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;

    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterICP;
    pcl::VoxelGrid<PointType>
        downSizeFilterSurroundingKeyPoses;  // for surrounding key poses of
                                            // scan-to-map optimization

    nano_gicp::NanoGICP<PointType, PointType> gicpScan2Scan;

    ros::Time timeLaserInfoStamp;
    double timeLaserInfoCur;

    float transformTobeMapped[6];

    std::mutex mtx;

    bool isDegenerate = false;
    cv::Mat matP;

    int laserCloudCornerLastDSNum = 0;
    int laserCloudSurfLastDSNum = 0;

    bool aLoopIsClosed = false;
    int imuPreintegrationResetId = 0;

    nav_msgs::Path globalPath;

    Eigen::Affine3f transPointAssociateToMap;

    sensor_fusion::cloud_info cloudInfo;

    lidarOdometry() {
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);

        pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>(
            PROJECT_NAME + "/lidar/odometry/trajectory", 1);
        pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>(
            PROJECT_NAME + "/lidar/odometry/map_global", 1);
        pubOdomAftMappedROS = nh.advertise<nav_msgs::Odometry>(
            PROJECT_NAME + "/lidar/odometry/odometry", 1);
        pubPath = nh.advertise<nav_msgs::Path>(
            PROJECT_NAME + "/lidar/odometry/path", 1);

        pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>(
            PROJECT_NAME + "/lidar/odometry/map_local", 1);
        pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>(
            PROJECT_NAME + "/lidar/odometry/cloud_registered", 1);
        pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>(
            PROJECT_NAME + "/lidar/odometry/cloud_registered_raw", 1);

        downSizeFilterCorner.setLeafSize(mappingCornerLeafSize,
                                         mappingCornerLeafSize,
                                         mappingCornerLeafSize);
        downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize,
                                       mappingSurfLeafSize);
        downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize,
                                      mappingSurfLeafSize);
        downSizeFilterSurroundingKeyPoses.setLeafSize(
            surroundingKeyframeDensity, surroundingKeyframeDensity,
            surroundingKeyframeDensity);  // for surrounding key poses of
                                          // scan-to-map optimization

        // GICP
        gicpScan2Scan.setCorrespondenceRandomness(10);
        gicpScan2Scan.setMaxCorrespondenceDistance(1.0);
        gicpScan2Scan.setMaximumIterations(32);
        gicpScan2Scan.setTransformationEpsilon(0.01);
        gicpScan2Scan.setEuclideanFitnessEpsilon(0.01);
        gicpScan2Scan.setRANSACIterations(5);
        gicpScan2Scan.setRANSACOutlierRejectionThreshold(1.0);

        pcl::Registration<PointType, PointType>::KdTreeReciprocalPtr temp;
        gicpScan2Scan.setSearchMethodSource(temp, true);
        gicpScan2Scan.setSearchMethodTarget(temp, true);

        // Subscriber
        subImu = nh.subscribe<sensor_msgs::Imu>(
            imuTopic, 2000, &lidarOdometry::imuHandler, this,
            ros::TransportHints().tcpNoDelay());
        subLaserCloudInfo = nh.subscribe<sensor_fusion::cloud_info>(
            PROJECT_NAME + "/lidar/feature/cloud_info", 5,
            &lidarOdometry::laserCloudInfoHandler, this,
            ros::TransportHints().tcpNoDelay());

        allocateMemory();
    }

    void allocateMemory() {
        cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());

        kdtreeSurroundingKeyPoses.reset(
            new nanoflann::KdTreeFLANN<PointType>());
        kdtreeHistoryKeyPoses.reset(new nanoflann::KdTreeFLANN<PointType>());

        laserCloudCornerLast.reset(
            new pcl::PointCloud<PointType>());  // corner feature set from
                                                // odoOptimization
        laserCloudSurfLast.reset(
            new pcl::PointCloud<PointType>());  // surf feature set from
                                                // odoOptimization
        laserCloudCornerLastDS.reset(
            new pcl::PointCloud<PointType>());  // downsampled corner featuer
                                                // set from odoOptimization
        laserCloudSurfLastDS.reset(
            new pcl::PointCloud<PointType>());  // downsampled surf featuer set
                                                // from odoOptimization

        laserCloudOri.reset(new pcl::PointCloud<PointType>());
        coeffSel.reset(new pcl::PointCloud<PointType>());

        laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);

        std::fill(laserCloudOriCornerFlag.begin(),
                  laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(),
                  false);

        laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());

        kdtreeCornerFromMap.reset(new nanoflann::KdTreeFLANN<PointType>());
        kdtreeSurfFromMap.reset(new nanoflann::KdTreeFLANN<PointType>());

        latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());

        for (int i = 0; i < 6; ++i) {
            transformTobeMapped[i] = 0;
        }

        matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
    }

    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg) {}

    void laserCloudInfoHandler(const sensor_fusion::cloud_infoConstPtr& msgIn) {
        // extract time stamp
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();

        // extract info ana feature cloud
        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

        std::lock_guard<std::mutex> lock(mtx);

        static double timeLastProcessing = -1;
        // if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
        // {
        timeLastProcessing = timeLaserInfoCur;

        downsampleCurrentScan();

        updateInitialGuess();

        extractSurroundingKeyFrames();

        scan2MapOptimization();

        saveKeyFramesAndFactor();

        publishOdometry();

        publishFrames();
        // }
    }

    void pointAssociateToMap(PointType const* const pi, PointType* const po) {
        po->x = transPointAssociateToMap(0, 0) * pi->x +
                transPointAssociateToMap(0, 1) * pi->y +
                transPointAssociateToMap(0, 2) * pi->z +
                transPointAssociateToMap(0, 3);
        po->y = transPointAssociateToMap(1, 0) * pi->x +
                transPointAssociateToMap(1, 1) * pi->y +
                transPointAssociateToMap(1, 2) * pi->z +
                transPointAssociateToMap(1, 3);
        po->z = transPointAssociateToMap(2, 0) * pi->x +
                transPointAssociateToMap(2, 1) * pi->y +
                transPointAssociateToMap(2, 2) * pi->z +
                transPointAssociateToMap(2, 3);
        po->intensity = pi->intensity;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(
        pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn) {
        pcl::PointCloud<PointType>::Ptr cloudOut(
            new pcl::PointCloud<PointType>());

        PointType* pointFrom;

        int cloudSize = cloudIn->size();
        cloudOut->resize(cloudSize);

        Eigen::Affine3f transCur = pcl::getTransformation(
            transformIn->x, transformIn->y, transformIn->z, transformIn->roll,
            transformIn->pitch, transformIn->yaw);

        for (int i = 0; i < cloudSize; ++i) {
            pointFrom = &cloudIn->points[i];
            cloudOut->points[i].x =
                transCur(0, 0) * pointFrom->x + transCur(0, 1) * pointFrom->y +
                transCur(0, 2) * pointFrom->z + transCur(0, 3);
            cloudOut->points[i].y =
                transCur(1, 0) * pointFrom->x + transCur(1, 1) * pointFrom->y +
                transCur(1, 2) * pointFrom->z + transCur(1, 3);
            cloudOut->points[i].z =
                transCur(2, 0) * pointFrom->x + transCur(2, 1) * pointFrom->y +
                transCur(2, 2) * pointFrom->z + transCur(2, 3);
            cloudOut->points[i].intensity = pointFrom->intensity;
        }
        return cloudOut;
    }

    gtsam::Pose3 affine3fTogtsamPose3(const Eigen::Affine3f& thisPose) {
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(thisPose, x, y, z, roll, pitch, yaw);
        return gtsam::Pose3(
            gtsam::Rot3::RzRyRx(double(roll), double(pitch), double(yaw)),
            gtsam::Point3(double(x), double(y), double(z)));
    }

    gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) {
        return gtsam::Pose3(
            gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch),
                                double(thisPoint.yaw)),
            gtsam::Point3(double(thisPoint.x), double(thisPoint.y),
                          double(thisPoint.z)));
    }

    gtsam::Pose3 trans2gtsamPose(float transformIn[]) {
        return gtsam::Pose3(
            gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
            gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
    }

    Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint) {
        return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z,
                                      thisPoint.roll, thisPoint.pitch,
                                      thisPoint.yaw);
    }

    Eigen::Affine3f trans2Affine3f(float transformIn[]) {
        return pcl::getTransformation(transformIn[3], transformIn[4],
                                      transformIn[5], transformIn[0],
                                      transformIn[1], transformIn[2]);
    }

    void trans2floatPose(const gtsam::Pose3& pose, float transformIn[]) {
        transformIn[0] = pose.rotation().roll();
        transformIn[1] = pose.rotation().pitch();
        transformIn[2] = pose.rotation().yaw();
        transformIn[3] = pose.translation().x();
        transformIn[4] = pose.translation().y();
        transformIn[5] = pose.translation().z();
    }

    PointTypePose trans2PointTypePose(float transformIn[]) {
        PointTypePose thisPose6D;
        thisPose6D.x = transformIn[3];
        thisPose6D.y = transformIn[4];
        thisPose6D.z = transformIn[5];
        thisPose6D.roll = transformIn[0];
        thisPose6D.pitch = transformIn[1];
        thisPose6D.yaw = transformIn[2];
        return thisPose6D;
    }

    void publishGlobalMap() {
        if (pubLaserCloudSurround.getNumSubscribers() == 0)
            return;

        if (cloudKeyPoses3D->points.empty() == true)
            return;

        nanoflann::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(
            new nanoflann::KdTreeFLANN<PointType>());
        ;
        pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(
            new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(
            new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(
            new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(
            new pcl::PointCloud<PointType>());

        // kd-tree to find near key frames to visualize
        std::vector<int> pointSearchIndGlobalMap;
        std::vector<float> pointSearchSqDisGlobalMap;
        // search near key frames to visualize
        mtx.lock();
        kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
        kdtreeGlobalMap->radiusSearch(
            cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius,
            pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
        mtx.unlock();

        for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
            globalMapKeyPoses->push_back(
                cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
        // downsample near selected key frames
        pcl::VoxelGrid<PointType>
            downSizeFilterGlobalMapKeyPoses;  // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setLeafSize(
            globalMapVisualizationPoseDensity,
            globalMapVisualizationPoseDensity,
            globalMapVisualizationPoseDensity);  // for global map visualization
        downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
        downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);

        // extract visualized and downsampled key frames
        for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i) {
            if (pointDistance(globalMapKeyPosesDS->points[i],
                              cloudKeyPoses3D->back()) >
                globalMapVisualizationSearchRadius)
                continue;
            int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
            *globalMapKeyFrames +=
                *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],
                                     &cloudKeyPoses6D->points[thisKeyInd]);
            *globalMapKeyFrames +=
                *transformPointCloud(surfCloudKeyFrames[thisKeyInd],
                                     &cloudKeyPoses6D->points[thisKeyInd]);
        }
        // downsample visualized points
        pcl::VoxelGrid<PointType>
            downSizeFilterGlobalMapKeyFrames;  // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setLeafSize(
            globalMapVisualizationLeafSize, globalMapVisualizationLeafSize,
            globalMapVisualizationLeafSize);  // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
        downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
        publishCloud(&pubLaserCloudSurround, *globalMapKeyFramesDS,
                     timeLaserInfoStamp, "odom");
    }

    void updateInitialGuess() {
        static Eigen::Affine3f lastImuTransformation;
        // system initialization
        if (cloudKeyPoses3D->points.empty()) {
            if (cloudInfo.imuRollInit == -9999 &&
                cloudInfo.imuPitchInit == -9999 &&
                cloudInfo.imuPitchInit == -9999) {
                transformTobeMapped[0] = 0;
                transformTobeMapped[1] = 0;
                transformTobeMapped[2] = 0;

                lastImuTransformation = pcl::getTransformation(
                    0, 0, 0, transformTobeMapped[0], transformTobeMapped[1],
                    transformTobeMapped[2]);  // save imu before return;
            } else {
                transformTobeMapped[0] = cloudInfo.imuRollInit;
                transformTobeMapped[1] = cloudInfo.imuPitchInit;
                transformTobeMapped[2] = cloudInfo.imuYawInit;

                if (!useImuHeadingInitialization)
                    transformTobeMapped[2] = 0;

                lastImuTransformation = pcl::getTransformation(
                    0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
                    cloudInfo.imuYawInit);  // save imu before return;
            }

            pcl::PointCloud<PointType>::Ptr target (new pcl::PointCloud<PointType>);
            *target = *laserCloudSurfLastDS;
            gicpScan2Scan.setInputTarget(target);

            return;
        }

        // use VINS odometry estimation for pose guess
        static int odomResetId = 0;
        static bool lastVinsTransAvailable = false;
        static Eigen::Affine3f lastVinsTransformation;
        if (cloudInfo.odomAvailable == true &&
            cloudInfo.odomResetId == odomResetId) {
            // ROS_INFO("Using VINS initial guess");
            if (lastVinsTransAvailable == false) {
                // ROS_INFO("Initializing VINS initial guess");
                lastVinsTransformation = pcl::getTransformation(
                    cloudInfo.odomX, cloudInfo.odomY, cloudInfo.odomZ,
                    cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);
                lastVinsTransAvailable = true;
            } else {
                // ROS_INFO("Obtaining VINS incremental guess");
                Eigen::Affine3f transBack = pcl::getTransformation(
                    cloudInfo.odomX, cloudInfo.odomY, cloudInfo.odomZ,
                    cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);
                Eigen::Affine3f transIncre =
                    lastVinsTransformation.inverse() * transBack;

                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                Eigen::Affine3f transFinal = transTobe * transIncre;
                pcl::getTranslationAndEulerAngles(
                    transFinal, transformTobeMapped[3], transformTobeMapped[4],
                    transformTobeMapped[5], transformTobeMapped[0],
                    transformTobeMapped[1], transformTobeMapped[2]);

                lastVinsTransformation = pcl::getTransformation(
                    cloudInfo.odomX, cloudInfo.odomY, cloudInfo.odomZ,
                    cloudInfo.odomRoll, cloudInfo.odomPitch, cloudInfo.odomYaw);

                lastImuTransformation = pcl::getTransformation(
                    0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
                    cloudInfo.imuYawInit);  // save imu before return;
                return;
            }
        } else {
            // ROS_WARN("VINS failure detected.");
            lastVinsTransAvailable = false;
            odomResetId = cloudInfo.odomResetId;
        }

        // use imu incremental estimation for pose guess (only rotation)
        if (cloudInfo.imuAvailable == true) {
            // ROS_INFO("Using IMU initial guess");
            Eigen::Affine3f transBack = pcl::getTransformation(
                0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
                cloudInfo.imuYawInit);
            Eigen::Affine3f transIncre =
                lastImuTransformation.inverse() * transBack;

            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(
                transFinal, transformTobeMapped[3], transformTobeMapped[4],
                transformTobeMapped[5], transformTobeMapped[0],
                transformTobeMapped[1], transformTobeMapped[2]);

            lastImuTransformation = pcl::getTransformation(
                0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit,
                cloudInfo.imuYawInit);  // save imu before return;
            return;
        }

        {
            pcl::PointCloud<PointType>::Ptr source (new pcl::PointCloud<PointType>);
            *source = *laserCloudSurfLastDS;
            gicpScan2Scan.setInputSource(source);
            pcl::PointCloud<PointType>::Ptr aligned (new pcl::PointCloud<PointType>);
            gicpScan2Scan.align(*aligned);

            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * Eigen::Affine3f(gicpScan2Scan.getFinalTransformation());

            pcl::getTranslationAndEulerAngles(
                transFinal, transformTobeMapped[3], transformTobeMapped[4],
                transformTobeMapped[5], transformTobeMapped[0],
                transformTobeMapped[1], transformTobeMapped[2]);

            gicpScan2Scan.swapSourceAndTarget();
            return;
        }
        
    }

    void extractNearby() {
        pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(
            new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(
            new pcl::PointCloud<PointType>());
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        // extract all the nearby key poses and downsample them
        kdtreeSurroundingKeyPoses->setInputCloud(
            cloudKeyPoses3D);  // create kd-tree
        kdtreeSurroundingKeyPoses->radiusSearch(
            cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius,
            pointSearchInd, pointSearchSqDis);
        for (int i = 0; i < (int)pointSearchInd.size(); ++i) {
            int id = pointSearchInd[i];
            surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
        }

        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);

        // also extract some latest key frames in case the robot rotates in one
        // position
        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses - 1; i >= 0; --i) {
            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
            else
                break;
        }

        extractCloud(surroundingKeyPosesDS);
    }

    void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract) {
        std::vector<pcl::PointCloud<PointType>> laserCloudCornerSurroundingVec;
        std::vector<pcl::PointCloud<PointType>> laserCloudSurfSurroundingVec;

        laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
        laserCloudSurfSurroundingVec.resize(cloudToExtract->size());

// extract surrounding map
#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < (int)cloudToExtract->size(); ++i) {
            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            if (pointDistance(cloudKeyPoses3D->points[thisKeyInd],
                              cloudKeyPoses3D->back()) >
                surroundingKeyframeSearchRadius)
                continue;
            laserCloudCornerSurroundingVec[i] =
                *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],
                                     &cloudKeyPoses6D->points[thisKeyInd]);
            laserCloudSurfSurroundingVec[i] =
                *transformPointCloud(surfCloudKeyFrames[thisKeyInd],
                                     &cloudKeyPoses6D->points[thisKeyInd]);
        }

        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();
        for (int i = 0; i < (int)cloudToExtract->size(); ++i) {
            *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
            *laserCloudSurfFromMap += laserCloudSurfSurroundingVec[i];
        }

        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
    }

    void extractSurroundingKeyFrames() {
        if (cloudKeyPoses3D->points.empty() == true)
            return;

        extractNearby();
    }

    void downsampleCurrentScan() {
        // Downsample cloud from current scan
        laserCloudCornerLastDS->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
        downSizeFilterCorner.filter(*laserCloudCornerLastDS);
        laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();

        laserCloudSurfLastDS->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
        downSizeFilterSurf.filter(*laserCloudSurfLastDS);
        laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
    }

    void updatePointAssociateToMap() {
        transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
    }

    void scan2MapOptimization() {
        if (cloudKeyPoses3D->points.empty())
            return;

        if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum &&
            laserCloudSurfLastDSNum > surfFeatureMinValidNum) {
            TicToc t_tree;
            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
            if (debugTime) {
                std::printf("build tree time %f ms \n", t_tree.toc());
            }

            {
                TicToc t_opt;
                kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
                kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);

                for (int iterCount = 0; iterCount < interOptNum; iterCount++) {
                    // create a gtsam graph
                    gtsam::NonlinearFactorGraph::shared_ptr graph(
                        new gtsam::NonlinearFactorGraph());
                    gtsam::Values initial_estimate;
                    gtsam::Pose3 init = trans2gtsamPose(transformTobeMapped);
                    initial_estimate.insert(0, init);

                    TicToc t_data;
                    double t_search_nearest = 0;

                    int corner_num = 0;
                    for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
                        const auto& pointOri =
                            laserCloudCornerLastDS->points[i];
                        // double sqrtDis = pointOri.x * pointOri.x + pointOri.y
                        // * pointOri.y + pointOri.z * pointOri.z;
                        PointType pointSel;
                        pointAssociateToMap(&pointOri, &pointSel);

                        TicToc t_search;
                        std::vector<int> pointSearchInd;
                        std::vector<float> pointSearchSqDis;
                        kdtreeCornerFromMap->nearestKSearch(
                            pointSel, 5, pointSearchInd, pointSearchSqDis);
                        t_search_nearest += t_search.toc();

                        if (pointSearchSqDis[4] < 1.0) {
                            std::vector<Eigen::Vector3d> nearCorners;
                            Eigen::Vector3d center(0, 0, 0);
                            for (int j = 0; j < 5; j++) {
                                Eigen::Vector3d tmp(
                                    laserCloudCornerFromMap
                                        ->points[pointSearchInd[j]]
                                        .x,
                                    laserCloudCornerFromMap
                                        ->points[pointSearchInd[j]]
                                        .y,
                                    laserCloudCornerFromMap
                                        ->points[pointSearchInd[j]]
                                        .z);
                                center = center + tmp;
                                nearCorners.push_back(tmp);
                            }
                            center = center / 5.0;

                            Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
                            for (int j = 0; j < 5; j++) {
                                Eigen::Matrix<double, 3, 1> tmpZeroMean =
                                    nearCorners[j] - center;
                                covMat = covMat +
                                         tmpZeroMean * tmpZeroMean.transpose();
                            }

                            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(
                                covMat);

                            // if is indeed line feature note Eigen library sort
                            // eigenvalues in increasing order
                            Eigen::Vector3d unit_direction =
                                saes.eigenvectors().col(2);
                            Eigen::Vector3d curr_point(pointOri.x, pointOri.y,
                                                       pointOri.z);
                            if (saes.eigenvalues()[2] >
                                3 * saes.eigenvalues()[1]) {
                                Eigen::Vector3d point_on_line = center;
                                Eigen::Vector3d point_a, point_b;
                                point_a = 0.1 * unit_direction + point_on_line;
                                point_b = -0.1 * unit_direction + point_on_line;

                                gtsam::noiseModel::Diagonal::shared_ptr
                                    edge_gaussian_model =
                                        gtsam::noiseModel::Diagonal::Variances(
                                            (gtsam::Vector(3) << 1.0, 1.0, 1.0)
                                                .finished());
                                gtsam::noiseModel::Robust::shared_ptr
                                    edge_noise_model =
                                        gtsam::noiseModel::Robust::Create(
                                            gtsam::noiseModel::mEstimator::
                                                Huber::Create(0.1),
                                            edge_gaussian_model);
                                gtsam::LidarPose3EdgeFactor factor(
                                    0, curr_point, point_a, point_b, 1.0,
                                    edge_noise_model);

                                graph->push_back(factor);
                                corner_num++;
                            }
                        }
                    }

                    int surf_num = 0;
                    for (int i = 0; i < laserCloudSurfLastDSNum; i++) {
                        const auto& pointOri = laserCloudSurfLastDS->points[i];
                        // double sqrtDis = pointOri.x * pointOri.x + pointOri.y
                        // * pointOri.y + pointOri.z * pointOri.z;
                        PointType pointSel;
                        pointAssociateToMap(&pointOri, &pointSel);

                        TicToc t_search;
                        std::vector<int> pointSearchInd;
                        std::vector<float> pointSearchSqDis;
                        kdtreeSurfFromMap->nearestKSearch(
                            pointSel, 5, pointSearchInd, pointSearchSqDis);
                        t_search_nearest += t_search.toc();

                        Eigen::Matrix<double, 5, 3> matA0;
                        Eigen::Matrix<double, 5, 1> matB0 =
                            -1 * Eigen::Matrix<double, 5, 1>::Ones();
                        if (pointSearchSqDis[4] < 1.0) {
                            for (int j = 0; j < 5; j++) {
                                matA0(j, 0) = laserCloudSurfFromMap
                                                  ->points[pointSearchInd[j]]
                                                  .x;
                                matA0(j, 1) = laserCloudSurfFromMap
                                                  ->points[pointSearchInd[j]]
                                                  .y;
                                matA0(j, 2) = laserCloudSurfFromMap
                                                  ->points[pointSearchInd[j]]
                                                  .z;
                            }
                            // find the norm of plane
                            Eigen::Vector3d norm =
                                matA0.colPivHouseholderQr().solve(matB0);
                            double negative_OA_dot_norm = 1 / norm.norm();
                            norm.normalize();

                            // Here n(pa, pb, pc) is unit norm of plane
                            bool planeValid = true;
                            for (int j = 0; j < 5; j++) {
                                // if OX * n > 0.2, then plane is not fit well
                                if (fabs(norm(0) *
                                             laserCloudSurfFromMap
                                                 ->points[pointSearchInd[j]]
                                                 .x +
                                         norm(1) *
                                             laserCloudSurfFromMap
                                                 ->points[pointSearchInd[j]]
                                                 .y +
                                         norm(2) *
                                             laserCloudSurfFromMap
                                                 ->points[pointSearchInd[j]]
                                                 .z +
                                         negative_OA_dot_norm) > 0.2) {
                                    planeValid = false;
                                    break;
                                }
                            }
                            Eigen::Vector3d curr_point(pointOri.x, pointOri.y,
                                                       pointOri.z);
                            if (planeValid) {
                                gtsam::noiseModel::Diagonal::shared_ptr
                                    surf_gaussian_model =
                                        gtsam::noiseModel::Diagonal::Variances(
                                            (gtsam::Vector(1) << 1.0)
                                                .finished());
                                gtsam::noiseModel::Robust::shared_ptr
                                    surf_noise_model =
                                        gtsam::noiseModel::Robust::Create(
                                            gtsam::noiseModel::mEstimator::
                                                Huber::Create(0.1),
                                            surf_gaussian_model);
                                gtsam::LidarPose3PlaneNormFactor factor(
                                    0, curr_point, norm, negative_OA_dot_norm,
                                    surf_noise_model);

                                graph->push_back(factor);
                                surf_num++;
                            }
                        }
                    }

                    if (debugTime) {
                        std::printf("corner num %d used corner num %d \n",
                                    laserCloudCornerLastDSNum, corner_num);
                        std::printf("surf num %d used surf num %d \n",
                                    laserCloudSurfLastDSNum, surf_num);
                        std::printf("mapping data assosiation time %f ms \n",
                                    t_data.toc());
                    }

                    TicToc t_solver;

                    gtsam::LevenbergMarquardtParams optimizer_params;
                    optimizer_params.setLinearSolverType("MULTIFRONTAL_QR");
                    optimizer_params.setRelativeErrorTol(1e-4);

                    gtsam::LevenbergMarquardtOptimizer optimizer(
                        *graph, initial_estimate, optimizer_params);

                    gtsam::Values result = optimizer.optimize();
                    auto currPose = result.at<gtsam::Pose3>(0);
                    trans2floatPose(currPose, transformTobeMapped);
                    updatePointAssociateToMap();

                    if (debugTime) {
                        std::printf("mapping solver time %f ms \n",
                                    t_solver.toc());
                    }
                }
            }

            transformUpdate();
        } else {
            ROS_WARN(
                "Not enough features! Only %d edge and %d planar features "
                "available.",
                laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
        }
    }

    void transformUpdate() {
        transformTobeMapped[0] = constraintTransformation(
            transformTobeMapped[0], rotation_tollerance);
        transformTobeMapped[1] = constraintTransformation(
            transformTobeMapped[1], rotation_tollerance);
        transformTobeMapped[5] =
            constraintTransformation(transformTobeMapped[5], z_tollerance);
    }

    float constraintTransformation(float value, float limit) {
        if (value < -limit)
            value = -limit;
        if (value > limit)
            value = limit;

        return value;
    }

    bool saveFrame() {
        if (cloudKeyPoses3D->points.empty())
            return true;

        Eigen::Affine3f transStart =
            pclPointToAffine3f(cloudKeyPoses6D->back());
        Eigen::Affine3f transFinal = pcl::getTransformation(
            transformTobeMapped[3], transformTobeMapped[4],
            transformTobeMapped[5], transformTobeMapped[0],
            transformTobeMapped[1], transformTobeMapped[2]);
        Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch,
                                          yaw);

        if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
            abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
            abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
            sqrt(x * x + y * y + z * z) <
                surroundingkeyframeAddingDistThreshold)
            return false;

        return true;
    }

    void saveKeyFramesAndFactor() {
        if (saveFrame() == false)
            return;

        // // odom factor
        // addOdomFactor();

        // // gps factor
        // // addGPSFactor();

        // // loop factor
        // addLoopFactor();

        // update iSAM
        // isam->update(gtSAMgraph, initialEstimate);
        // isam->update();

        // gtSAMgraph.resize(0);
        // initialEstimate.clear();

        // save key poses
        PointType thisPose3D;
        PointTypePose thisPose6D;
        gtsam::Pose3 latestEstimate = trans2gtsamPose(transformTobeMapped);

        // isamCurrentEstimate = isam->calculateEstimate();
        // latestEstimate =
        //     isamCurrentEstimate.at<gtsam::Pose3>(isamCurrentEstimate.size() -
        //     1);
        // cout << "****************************************************" <<
        // endl; isamCurrentEstimate.print("Current estimate: ");

        thisPose3D.x = latestEstimate.translation().x();
        thisPose3D.y = latestEstimate.translation().y();
        thisPose3D.z = latestEstimate.translation().z();
        thisPose3D.intensity =
            cloudKeyPoses3D->size();  // this can be used as index
        cloudKeyPoses3D->push_back(thisPose3D);

        thisPose6D.x = thisPose3D.x;
        thisPose6D.y = thisPose3D.y;
        thisPose6D.z = thisPose3D.z;
        thisPose6D.intensity =
            thisPose3D.intensity;  // this can be used as index
        thisPose6D.roll = latestEstimate.rotation().roll();
        thisPose6D.pitch = latestEstimate.rotation().pitch();
        thisPose6D.yaw = latestEstimate.rotation().yaw();
        thisPose6D.time = timeLaserInfoCur;
        cloudKeyPoses6D->push_back(thisPose6D);

        // cout << "****************************************************" <<
        // endl; cout << "Pose covariance:" << endl; cout <<
        // isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl <<
        // endl;
        // poseCovariance =
        //     isam->marginalCovariance(isamCurrentEstimate.size() - 1);

        // save updated transform
        trans2floatPose(latestEstimate, transformTobeMapped);
        // transformTobeMapped[0] = latestEstimate.rotation().roll();
        // transformTobeMapped[1] = latestEstimate.rotation().pitch();
        // transformTobeMapped[2] = latestEstimate.rotation().yaw();
        // transformTobeMapped[3] = latestEstimate.translation().x();
        // transformTobeMapped[4] = latestEstimate.translation().y();
        // transformTobeMapped[5] = latestEstimate.translation().z();

        // save all the received edge and surf points
        pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(
            new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(
            new pcl::PointCloud<PointType>());
        pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
        pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);

        // save key frame cloud
        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);

        // save path for visualization
        updatePath(thisPose6D);
    }

    void publishOdometry() {
        // Publish odometry for ROS
        nav_msgs::Odometry laserOdometryROS;
        laserOdometryROS.header.stamp = timeLaserInfoStamp;
        laserOdometryROS.header.frame_id = "odom";
        laserOdometryROS.child_frame_id = "odom_mapping";
        laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
        laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
        laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
        laserOdometryROS.pose.pose.orientation =
            tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0],
                                                    transformTobeMapped[1],
                                                    transformTobeMapped[2]);
        laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId);
        pubOdomAftMappedROS.publish(laserOdometryROS);
        // Publish TF
        static tf::TransformBroadcaster br;
        tf::Transform t_odom_to_lidar = tf::Transform(
            tf::createQuaternionFromRPY(transformTobeMapped[0],
                                        transformTobeMapped[1],
                                        transformTobeMapped[2]),
            tf::Vector3(transformTobeMapped[3], transformTobeMapped[4],
                        transformTobeMapped[5]));
        tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(
            t_odom_to_lidar, timeLaserInfoStamp, "odom", "lidar_link");
        br.sendTransform(trans_odom_to_lidar);
    }

    void updatePath(const PointTypePose& pose_in) {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
        pose_stamped.header.frame_id = "odom";
        pose_stamped.pose.position.x = pose_in.x;
        pose_stamped.pose.position.y = pose_in.y;
        pose_stamped.pose.position.z = pose_in.z;
        tf::Quaternion q = tf::createQuaternionFromRPY(
            pose_in.roll, pose_in.pitch, pose_in.yaw);
        pose_stamped.pose.orientation.x = q.x();
        pose_stamped.pose.orientation.y = q.y();
        pose_stamped.pose.orientation.z = q.z();
        pose_stamped.pose.orientation.w = q.w();

        globalPath.poses.push_back(pose_stamped);
    }

    void publishFrames() {
        if (cloudKeyPoses3D->points.empty())
            return;
        // publish key poses
        publishCloud(&pubKeyPoses, *cloudKeyPoses6D, timeLaserInfoStamp,
                     "odom");
        // Publish surrounding key frames
        publishCloud(&pubRecentKeyFrames, *laserCloudSurfFromMapDS,
                     timeLaserInfoStamp, "odom");
        // publish registered key frame
        if (pubRecentKeyFrame.getNumSubscribers() != 0) {
            pcl::PointCloud<PointType>::Ptr cloudOut(
                new pcl::PointCloud<PointType>());
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut +=
                *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
            *cloudOut +=
                *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
            publishCloud(&pubRecentKeyFrame, *cloudOut, timeLaserInfoStamp,
                         "odom");
        }
        // publish registered high-res raw cloud
        if (pubCloudRegisteredRaw.getNumSubscribers() != 0) {
            pcl::PointCloud<PointType>::Ptr cloudOut(
                new pcl::PointCloud<PointType>());
            pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
            publishCloud(&pubCloudRegisteredRaw, *cloudOut, timeLaserInfoStamp,
                         "odom");
        }
        // publish path
        if (pubPath.getNumSubscribers() != 0) {
            globalPath.header.stamp = timeLaserInfoStamp;
            globalPath.header.frame_id = "odom";
            pubPath.publish(globalPath);
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "lidar");

    lidarOdometry LO;

    ROS_INFO("\033[1;32m----> Lidar Map Optimization Started.\033[0m");

    ros::spin();

    return 0;
}
